/* Encoder Library - TwoKnobs Example
 * http://www.pjrc.com/teensy/td_libs_Encoder.html
 *
 * This example code is in the public domain.
 */

/*
  Orientation Examples
 Send PWM to actuator 0: force actuator to close finger
 Send PWM to actuator 255: force actuator to open finger
 
 Scaled values 0 to 100
 Encoder position at 0: finger closed
 Encoder position at 100: finger open
 */

#include <Encoder.h>
#include <PID_v1.h>

// Change these pin numbers to the pins connected to your encoder.
//   Best Performance: both pins have interrupt capability
//   Good Performance: only the first pin has interrupt capability
//   Low Performance:  neither pin has interrupt capability
Encoder actuator(18, 19); // 18 is red, 19 is black, tied to interrupt pins on MEGA
//   avoid using pins with LEDs attached
int c2=1;
unsigned long time;
unsigned long time2;
long encoder_range;
int ACT1 = 9; //pin 9 for actuator 1
int count = 0;

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,12,4,1, DIRECT);

long previousMillis = 0; //initialize at 0
long interval = 5; // milliseconds to update sin
long sincounter = 0; // input to sin: increment at a set interval

void setup() {
  Serial.begin(9600);
  Serial.println("Encoder Test:");
  Input = actuator.read();
  Setpoint = 90; // set point value of 80 is 80% open on scale of 0 - 100
  myPID.SetMode(AUTOMATIC);

  //attachInterrupt(0,sinGenerate,CHANGE);

  //TCCR3B = TCCR3B & 0b1111000 | 0x01; // 5 is on timer 3
  TCCR2B = TCCR2B & 0b1111000 | 0x01;

  //calibrate end points of claw range of motion, 
  //grab encoder values at these boundary points


  ramp(-1); // close finger
  actuator.write(0); //set initial minimum val from encoder
  Serial.print("read value (closed): ");
  Serial.println(actuator.read());

  ramp(1); // open finger
  Serial.print("read value (open): ");
  Serial.println(actuator.read());

  encoder_range = actuator.read();//set other end point, max val from encoder
  Serial.print("read value encoder range: ");
  Serial.println(encoder_range);

  analogWrite(ACT1,127); // stop applying force now that encoder range is calibrated (127 is middle of 0-255)

  myPID.SetOutputLimits(5, 250); // Guess: the actuator doesn't like a PWM of 0% or 100%?
  myPID.SetSampleTime(5); // Set the PID to compute every 5 ms
}

void loop() {

  long newAct;
  newAct = actuator.read();
  int writeval;
  Setpoint = 50+50*sinGenerate(); // 50 + 50* [-1,1] scales to [0,100]
  
  Input = newAct*100.0/encoder_range; //Scales from 0 to 100
  myPID.Compute();
  writeval = int(Output);
  analogWrite(ACT1,writeval); //check if 0 has issues
  //analogWrite(ACT1,127); //test value no force

  if (count ==1){
    Serial.print("Input value from encoder: ");
    Serial.println(Input);
    Serial.print("OUTPUT TO PWM ");
    Serial.println(writeval);
  }
  if(count == 10000){
    count = 0;  
  }
//  Serial.println("Working?");
  count++;
}

void ramp(int direction){ // 1: open, -1: close
  int STEPS = 100;

  for(int i = 0; i < STEPS/3; i++){ // Use < instead of <= ~~guess is that actuator doesn't like a PWM of 0% or 100%
    analogWrite(ACT1, 127 + i * direction * 127 / STEPS);
    delay(2000/STEPS); // 2 seconds total time
  }
  delay(500); // delay for testing
}

double sinGenerate(){
  //Returns the sine value at current time
  
  //long currentMillis = millis(); //get the time since program started
  //if(currentMillis - previousMillis > interval){ //If time elapsed is enough, update input to sin function
    sincounter++;
  //  previousMillis = currentMillis;
 // }
  return sin(sincounter*1.0/1000);
}



